//----------------------------------------------------------------------------------------------
//	Filename:	StructureGraph.h
//	Author:		Sandro Spina
//	Date:		27/01/2013
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------

#include "EdgeGraph.h"
#include "StructureGraphsForge.h"
#include "GraphCut.h"
#include <queue>
#include <boost\lexical_cast.hpp>

using namespace Illumina::Core;

EdgeGraph::EdgeGraph()
{		
	DisjointStructureGraphs = new Illumina::Core::List<Node*>();
	//ConsumedStates = new std::stack<std::vector<int>*>();
	DirectionsTravelled = new std::map<std::string, int>();
	nodeID = new ::UniqueID();
	nodes = new std::map<int, Node*>();
}

//Create and populate a new node to represent a surface segment. Add to nodes map.
void EdgeGraph::CreateNewNode(SegmentsGraph::SurfaceSegment * &p_ss)
{
	Node * node = new Node();
	node->id = p_ss->id;
	node->used = 0;
	node->stats = new NodeStats();
	node->stats->type = nodetype::accepting;
	node->stats->shape = EdgeGraph::shape::notset;
	node->stats->label = p_ss->label;		
	node->stats->point_count = p_ss->points->size();
	node->stats->bounding_min = p_ss->bounding_min;
	node->stats->bounding_max = p_ss->bounding_max;
	node->stats->mean_bounding_volume = (node->stats->bounding_min + node->stats->bounding_max) / 2;
	node->edges = new std::map<int, std::pair<EdgeStats*, Node*>*>();   //CHANGED FROM VECTOR TO MAP
	
	(*nodes)[p_ss->id] = node;
}	

void EdgeGraph::AddNodeConnection(SegmentsGraph::SurfaceSegment * &p_segment_source, SegmentsGraph::SurfaceSegment * &p_segment_sink, int p_strength) 
{
	Node * source = nodes->at(p_segment_source->id);
	Node * sink = nodes->at(p_segment_sink->id);
	EdgeStats * edge_stats = new EdgeStats();		
	edge_stats->label = "node";
	edge_stats->strength = p_strength;
	if (p_segment_source->shape_primitive == SegmentsGraph::line && p_segment_sink->shape_primitive == SegmentsGraph::line) 
	{
		source->stats->shape = EdgeGraph::shape::line;
		sink->stats->shape = EdgeGraph::shape::line;
		edge_stats->dot = Illumina::Core::Vector3::Dot(Illumina::Core::Vector3::Normalize(p_segment_source->bounding_max - p_segment_source->bounding_min), 
														Illumina::Core::Vector3::Normalize(p_segment_sink->bounding_max - p_segment_sink->bounding_min));
		edge_stats->epsilon = 0.1f;
	}				
	std::pair<EdgeStats*, Node*> * edge = new std::pair<EdgeStats*, Node*>(edge_stats, sink);
	if (source->edges->find(sink->id) != source->edges->end()) 
	{
		//Just increment strength		
		source->edges->find(sink->id)->second->first->strength+=p_strength;
		//std::cout << "Overwriting an EDGE !! << src=" << source->id << " snk=" << sink->id << " Incrementing strength to " << source->edges->find(sink->id)->second->first->strength << std::endl;
	}
	else
	{
		//Add new sink node
		(*source->edges)[sink->id] = edge;
	}
}

//Returns a list of indices in breadth first order	
Illumina::Core::List<int> * EdgeGraph::BreadthFirstTraversal(Node * p_initial)
{
	Illumina::Core::List<int> * traversal = new Illumina::Core::List<int>();
	std::map<int, bool> visited = std::map<int, bool>();
	for (nodesIterator = nodes->begin(); nodesIterator != nodes->end(); nodesIterator++)
	{
		visited[nodesIterator->first] = false;
	}

	std::queue<Node*> toVisit = std::queue<Node*>();
	toVisit.push(p_initial);
	while (!toVisit.empty())
	{
		traversal->PushBack(toVisit.front()->id);
		visited[toVisit.front()->id] = true;
		for (std::map<int, std::pair<EdgeStats*, Node*>*>::iterator toVisitIterator = toVisit.front()->edges->begin(); toVisitIterator != toVisit.front()->edges->end(); toVisitIterator++)
		//for (int i = 0; i < toVisit.front()->edges->size(); i++)
		{
			//if (!visited[toVisit.front()->edges->at(i)->second->id]) 
			if (!visited[toVisitIterator->first])
			{
				//toVisit.push(toVisit.front()->edges->at(i)->second);				
				//visited[toVisit.front()->edges->at(i)->second->id] = true;					
				toVisit.push(toVisitIterator->second->second);
				visited[toVisitIterator->first] = true;
			}
		}
		toVisit.pop();
	}
	return traversal;
}

/*
std::map<int,std::vector<GraphCut*>*> * EdgeGraph::SearchSceneGraph(std::string &query)
{
	StructureGraphsForge sgf;
	//Do some work(magic) here to transform the query into a structure graph.

	//Finished construction of structure graph
	return SearchSceneGraph(sgf.ForgeNullGraph());
}
*/

//Small states are merges with adjacent larger ones. Make sure graph remains consistent with regards to connectivity.
void EdgeGraph::MergeSmallStates(int p_min_number_of_points, SegmentsGraph * p_sg)
{	
	//Order surface nodes from biggest to smallest (still greater than p_min_number_of_points)
	std::vector<int> ordered_states; ordered_states.push_back(-1);
	std::vector<int> ordered_states_point_count; ordered_states_point_count.push_back(-1);
	for (std::map<int, Node*>::iterator states_iterator = nodes->begin(); states_iterator != nodes->end(); states_iterator++)
	{
		if (states_iterator->second->stats->point_count > p_min_number_of_points)
		{
			int insertion_idx=0;
			for (; insertion_idx<ordered_states.size(); insertion_idx++)
			{
				if (states_iterator->second->stats->point_count > ordered_states_point_count[insertion_idx])
					break;
			}
			ordered_states.insert(ordered_states.begin()+insertion_idx, states_iterator->first);
			ordered_states_point_count.insert(ordered_states_point_count.begin()+insertion_idx, states_iterator->second->stats->point_count);
		}
	}
	ordered_states.erase(ordered_states.end()-1); //Delete the initial -1 
	std::cout << "[MergeSmallStates] min_points= " << p_min_number_of_points << " #Big States = " << ordered_states.size() << " #Small States= " <<  nodes->size() - ordered_states.size() << std::endl;

	//Iterate over large states and 'eat'/merge smaller adjacent cells
	std::map<int, int> already_merged;
	std::vector<int> edges_to_delete;
	for (std::vector<int>::iterator ordered_iterator = ordered_states.begin(); ordered_iterator!=ordered_states.end(); ordered_iterator++)
	{
		Node* crt_state = nodes->at(*ordered_iterator);
		//Take in consideration transitive connections to small states.
		//Introduce while loop here ... Push transitive small states on a stack here. loop until stack is empty. $TODO$
		bool done = false;
		while (!done)
		{
			done = true;
			edges_to_delete.clear();
			for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator connected_states = crt_state->edges->begin(); connected_states != crt_state->edges->end(); connected_states++)
			{
				std::pair<EdgeStats*, Node*> * crt_edge = connected_states->second;
				if (crt_edge->second->stats->point_count <= p_min_number_of_points)
				{
					//Merge sink state at crt_edge with crt_state
					if (already_merged.find(connected_states->first) == already_merged.end())
					{
						Node* crt_state_to_prune = nodes->at(connected_states->first);
						//Start merging ... First transfer points from small to big state
						std::vector<int> * transfer_points_from = p_sg->m_surfaceSegments->at(connected_states->first)->points;
						std::vector<int> * transfer_points_to = p_sg->m_surfaceSegments->at(crt_state->id)->points;
						for (std::vector<int>::iterator pi = transfer_points_from->begin(); pi != transfer_points_from->end(); pi++)
						{
							transfer_points_to->push_back(*pi);
						}
						crt_state->stats->point_count = transfer_points_to->size();						
						transfer_points_from->clear();

						already_merged[connected_states->first] = 1;  //Mark as merged
						edges_to_delete.push_back(connected_states->first); //mark for deletion from current state
						//Add all outgoing edges of the pruned state at crt_edge to the crt_state						
						for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator to_prune_connected_states = crt_state_to_prune->edges->begin(); 
											to_prune_connected_states != crt_state_to_prune->edges->end(); to_prune_connected_states++)
						{
							if (to_prune_connected_states->first != *ordered_iterator && //not cyclic and
								crt_state->edges->find(connected_states->first) == crt_state->edges->end() && //not already an edge
								already_merged.find(to_prune_connected_states->first) != already_merged.end()) //not already pruned
							{							
								//This edge leads to a currently active state
								crt_state->edges->insert(std::pair<int,std::pair<EdgeStats*, Node*> *>(to_prune_connected_states->first, to_prune_connected_states->second));
								//Have we merged a small state ... 
								if (crt_state_to_prune->stats->point_count <= p_min_number_of_points) done = false;
								//std::cout << "State " << crt_state->id << " eating up state " << crt_state_to_prune->id << std::endl;
							}
						}
					} 
					else
					{
						//Just remove the edge ... state was already merged to larger state previously
						edges_to_delete.push_back(connected_states->first);
					}
				}
			}
			//Finally remove edges to states which have been pruned
			for (std::vector<int>::iterator tdi = edges_to_delete.begin(); tdi != edges_to_delete.end(); tdi++)
			{
				crt_state->edges->erase(crt_state->edges->find(*tdi));
			}
		}
	}

	//Finally delete from nodes map all pruned states
	for (std::map<int, int>::iterator pruned_iterator = already_merged.begin(); pruned_iterator != already_merged.end(); pruned_iterator++)
	{
		nodes->erase(nodes->find(pruned_iterator->first));
	}

	std::cout << "[MergeSmallStates] #States Pruned= " << already_merged.size() << " - #Big States= " << nodes->size() << std::endl;
}

void EdgeGraph::MergeBigStatesAcrossWeakEdges(int p_min_number_of_points, SegmentsGraph * p_sg)
{
	//Order surface nodes from biggest to smallest (still greater than p_min_number_of_points)
	std::vector<int> ordered_states; ordered_states.push_back(-1);
	std::vector<int> ordered_states_point_count; ordered_states_point_count.push_back(-1);
	for (std::map<int, Node*>::iterator states_iterator = nodes->begin(); states_iterator != nodes->end(); states_iterator++)
	{
		int insertion_idx=0;
		for (; insertion_idx<ordered_states.size(); insertion_idx++)
		{
			if (states_iterator->second->stats->point_count > ordered_states_point_count[insertion_idx])
				break;
		}
		ordered_states.insert(ordered_states.begin()+insertion_idx, states_iterator->first);
		ordered_states_point_count.insert(ordered_states_point_count.begin()+insertion_idx, states_iterator->second->stats->point_count);
	}
	
	ordered_states.erase(ordered_states.end()-1); //Delete the initial -1 
	std::cout << "[MergeBigStatesAcrossWeakEdges] min_points= " << p_min_number_of_points << " #Big States = " << ordered_states.size() << std::endl;

	//Iterate over states and merge with adjacent states if there's only one weak edge
	std::map<int, int> already_merged;
	std::vector<int> edges_to_delete;
	for (std::vector<int>::iterator ordered_iterator = ordered_states.begin(); ordered_iterator!=ordered_states.end(); ordered_iterator++)
	{
		Node* crt_state = nodes->at(*ordered_iterator);
		//Take in consideration transitive connections to small states.
		bool done = false;
		while (!done)
		{
			done = true;
			edges_to_delete.clear();
			for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator connected_states = crt_state->edges->begin(); connected_states != crt_state->edges->end(); connected_states++)
			{
				std::pair<EdgeStats*, Node*> * crt_edge = connected_states->second;
				if (crt_edge->second->stats->point_count <= p_min_number_of_points)
				{
					//Merge sink state at crt_edge with crt_state
					if (already_merged.find(connected_states->first) == already_merged.end())
					{
						Node* crt_state_to_prune = nodes->at(connected_states->first);
						//Start merging ... First transfer points from small to big state
						std::vector<int> * transfer_points_from = p_sg->m_surfaceSegments->at(connected_states->first)->points;
						std::vector<int> * transfer_points_to = p_sg->m_surfaceSegments->at(crt_state->id)->points;
						for (std::vector<int>::iterator pi = transfer_points_from->begin(); pi != transfer_points_from->end(); pi++)
						{
							transfer_points_to->push_back(*pi);
						}
						crt_state->stats->point_count = transfer_points_to->size();						
						transfer_points_from->clear();

						already_merged[connected_states->first] = 1;  //Mark as merged
						edges_to_delete.push_back(connected_states->first); //mark for deletion from current state
						//Add all outgoing edges of the pruned state at crt_edge to the crt_state						
						for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator to_prune_connected_states = crt_state_to_prune->edges->begin(); 
											to_prune_connected_states != crt_state_to_prune->edges->end(); to_prune_connected_states++)
						{
							if (to_prune_connected_states->first != *ordered_iterator && //not cyclic and
								crt_state->edges->find(connected_states->first) == crt_state->edges->end() && //not already an edge
								already_merged.find(to_prune_connected_states->first) != already_merged.end()) //not already pruned
							{							
								//This edge leads to a currently active state
								crt_state->edges->insert(std::pair<int,std::pair<EdgeStats*, Node*> *>(to_prune_connected_states->first, to_prune_connected_states->second));
								//Have we merged a small state ... 
								if (crt_state_to_prune->stats->point_count <= p_min_number_of_points) done = false;
								//std::cout << "State " << crt_state->id << " eating up state " << crt_state_to_prune->id << std::endl;
							}
						}
					} 
					else
					{
						//Just remove the edge ... state was already merged to larger state previously
						edges_to_delete.push_back(connected_states->first);
					}
				}
			}
			//Finally remove edges to states which have been pruned
			for (std::vector<int>::iterator tdi = edges_to_delete.begin(); tdi != edges_to_delete.end(); tdi++)
			{
				crt_state->edges->erase(crt_state->edges->find(*tdi));
			}
		}
	}

	//Finally delete from nodes map all pruned states
	for (std::map<int, int>::iterator pruned_iterator = already_merged.begin(); pruned_iterator != already_merged.end(); pruned_iterator++)
	{
		nodes->erase(nodes->find(pruned_iterator->first));
	}

	std::cout << "[MergeSmallStates] #States Pruned= " << already_merged.size() << " - #Big States= " << nodes->size() << std::endl;
}

void EdgeGraph::MergeBigStatesAcrossWeakEdges(SegmentsGraph * p_sg)
{
	//For each pair of states ... determine minimum number of points in edge

}

void EdgeGraph::PruneSmallStates(int p_min_number_of_points)
{
	//Iterate over all states and remove those which have less points than p_min_number_of_points
	std::vector<int> to_prune;
	
	//Determine states to prune
	for (std::map<int, Node*>::iterator states_iterator = nodes->begin(); states_iterator != nodes->end(); states_iterator++)
	{
		if (states_iterator->second->stats->point_count < p_min_number_of_points)
			to_prune.push_back(states_iterator->first);
	}

	//Remove edges to pruned states
	for (std::map<int, Node*>::iterator states_iterator = nodes->begin(); states_iterator != nodes->end(); states_iterator++)
	{
		for (std::vector<int>::iterator to_prune_iterator = to_prune.begin(); to_prune_iterator != to_prune.end(); to_prune_iterator++)
		{
			if (states_iterator->second->edges->find(*to_prune_iterator) != states_iterator->second->edges->end())
			{
				states_iterator->second->edges->erase(states_iterator->second->edges->find(*to_prune_iterator));
			}
		}
	}

	//Remove small states
	for (std::vector<int>::iterator to_prune_iterator = to_prune.begin(); to_prune_iterator != to_prune.end(); to_prune_iterator++)
	{
		nodes->erase(nodes->find(*to_prune_iterator));
	}
}

void EdgeGraph::PruneBigStates(int p_max_number_of_points)
{
	//Iterate over all states and remove those which have more points than p_min_number_of_points
	std::vector<int> to_prune;
	
	//Determine states to prune
	for (std::map<int, Node*>::iterator states_iterator = nodes->begin(); states_iterator != nodes->end(); states_iterator++)
	{
		if (states_iterator->second->stats->point_count > p_max_number_of_points)
			to_prune.push_back(states_iterator->first);
	}

	//Remove edges to pruned states
	for (std::map<int, Node*>::iterator states_iterator = nodes->begin(); states_iterator != nodes->end(); states_iterator++)
	{
		for (std::vector<int>::iterator to_prune_iterator = to_prune.begin(); to_prune_iterator != to_prune.end(); to_prune_iterator++)
		{
			if (states_iterator->second->edges->find(*to_prune_iterator) != states_iterator->second->edges->end())
			{
				states_iterator->second->edges->erase(states_iterator->second->edges->find(*to_prune_iterator));
			}
		}
	}

	//Remove big states
	for (std::vector<int>::iterator to_prune_iterator = to_prune.begin(); to_prune_iterator != to_prune.end(); to_prune_iterator++)
	{
		nodes->erase(nodes->find(*to_prune_iterator));
	}
}

void EdgeGraph::PruneWeakEdges(int p_min_number_of_points)
{
	//Iterate over all states and remove those which have less points than p_min_number_of_points
	std::vector<int> to_prune;
}

//RanSAC Methods over states ... Can be used to merge together or split adjacent states
bool EdgeGraph::RanSAC_FitCuboid(std::vector<int> p_states)
{
	return true;
}

bool EdgeGraph::RanSAC_FitPlane(std::vector<int> p_states)
{
	return true;
}

bool EdgeGraph::RanSAC_FitSphere(std::vector<int> p_states)
{
	return true;
}

bool EdgeGraph::RanSAC_FitCylinder(std::vector<int> p_states)
{
	return true;
}
	
//Shape Structure Graphs Search
std::vector<std::vector<int>> EdgeGraph::SearchCuboids()
{
	std::vector<std::vector<int>> cuboid_states;

	return cuboid_states;
}

std::vector<std::vector<int>> EdgeGraph::SearchPlanes()
{
	std::vector<std::vector<int>> plane_states;
	
	return plane_states;
}

std::vector<std::vector<int>> EdgeGraph::SearchSpheres()
{
	std::vector<std::vector<int>> sphere_states;

	return sphere_states;
}

std::vector<std::vector<int>> EdgeGraph::SearchCylinders()
{
	std::vector<std::vector<int>> cylinder_states;

	return cylinder_states;
}

std::vector<std::vector<int>> EdgeGraph::SearchObject(std::string p_shapeName)
{
	std::vector<std::vector<int>> object_states;

	return object_states;
}

//Graph Search Functions. map<node id, node>
//std::map<int,std::vector<std::vector<int>*>*> * SearchSceneGraph(std::pair<int, std::map<int, Node *>*> * structure)
/*
std::map<int,std::vector<GraphCut*>*> * EdgeGraph::SearchSceneGraph(ObjStructureGraph * structure)
{
	int initial = structure->GetInitialNodeId();
	std::map<int, ObjStructureGraph::Node*> * p_structureNodes = structure->GetNodes(); 

	//structures stores the graph cuts from the scene structure graph which match with the object input structure
	std::map<int, std::vector<std::vector<int>*>*> * structures = new std::map<int, std::vector<std::vector<int>*>*>();
	std::map<int, std::vector<GraphCut *> *> * structures2 = new std::map<int, std::vector<GraphCut *> *>();
		
	//stackTrace stores the suffix of the graph cut up to the point when the node is added to the stack
	std::stack<std::pair<int, int>> stack; 
	std::stack<std::pair<int, int>> stack2; //store the previous states to those in stack	
	
	std::stack<std::vector<int>> stackTrace; 
	std::stack<std::vector<int>> stackTraceObj; //the stack should keep trace of both the state and the outgoing transition  
	std::stack<std::vector<std::pair<std::pair<int,int>, int>>> stackTraceFull;

	std::stack<std::vector<int>> altClosedPaths; 
	std::stack<int> edgeStack;
		
	//store indices of current list of nodes making up this cut
	std::vector<int> crtGraphCut;
	std::vector<int> crtGraphCutObj;
	std::vector<int> crtGraphCutObjEdges;
	std::vector<ObjStructureGraph::Node *> crtGraphCutObjNodes;

	//std::vector<ObjStructureGraph::Node *> * crtGraphCutObjNodes = new std::vector<ObjStructureGraph::Node *>();

	ObjStructureGraph::EdgeStats * es;

	int prevSceneNodeId = 0;
	int prevStructureNodeId = 0;

	int crtStructureNodeId = initial;
	int crtSceneNodeId = 0;
	int crtStructureNodeEdgeId = -1;

	std::vector<std::pair<int, int>> * compatible = new std::vector<std::pair<int, int>>();
	std::vector<int> * edgeIndices = new std::vector<int>();

	std::map<std::string, bool> visitedpairs = std::map<std::string, bool>();
	std::map<int, bool> visitedstates = std::map<int, bool>();
	std::vector<int> tempStore = std::vector<int>();

	std::vector<ObjStructureGraph::action> * crtActions = new std::vector<ObjStructureGraph::action>();

	std::pair<int, int> crtPair;
	std::pair<int, int> prevPair;

	int structuresMapKey = 0;

	//search for stucture - naively iterate over all the nodes in the scene disjoint structure graphs for now.
	for (int dgraph = 0; dgraph < DisjointStructureGraphs->Size(); dgraph++)
	{
			//get an enumaration of the nodes in this scene disjoint structure graph
			Illumina::Core::List<int> * bft_nodes = BreadthFirstTraversal(DisjointStructureGraphs->At(dgraph));
			for (int nodeCount = 0; nodeCount < bft_nodes->Size(); nodeCount++)
			{
				//clear structures and add the next node to the stack ... which should initially (here) be empty
				if (nodes->at(bft_nodes->At(nodeCount))->used == true) 
				{						
					std::cout << "Skipping Consumed Node :: " << bft_nodes->At(nodeCount) << std::endl;
					continue;
				}
				visitedstates.clear();
				visitedpairs.clear();
				crtGraphCut.clear();
				crtGraphCutObj.clear();
				crtGraphCutObjEdges.clear();
				//crtGraphCutObjNodes->clear();
				GraphCut * crtGCut = new GraphCut(boost::lexical_cast<std::string>(initial));				
				stack.push(std::pair<int,int>(initial,bft_nodes->At(nodeCount)));
				stack2.push(std::pair<int,int>(initial,bft_nodes->At(nodeCount)));
				
				stackTrace.push(std::vector<int>());
				stackTraceObj.push(std::vector<int>());
				stackTraceFull.push(std::vector<std::pair<std::pair<int, int>, int>>());
				
				altClosedPaths.push(std::vector<int>());
				//crtGraphCut.push_back(bft_nodes->At(nodeCount));
				std::cout << "**************************************" << std::endl;
				std::cout << "Starting new search [" << initial << "," << bft_nodes->At(nodeCount) << "]" << std::endl;
				std::vector<std::vector<int>*>* GraphCutsAtNode = new std::vector<std::vector<int>*>();
				std::vector<GraphCut *> * GCutsAtNode = new std::vector<GraphCut *>();
				structuresMapKey = bft_nodes->At(nodeCount);

				while (!stack.empty())
				{
					//Get the next element from the stack
					crtPair = stack.top();	stack.pop();
					prevPair = stack2.top();  stack2.pop();
					prevSceneNodeId = prevPair.second;
					prevStructureNodeId = prevPair.first;					
					crtSceneNodeId = crtPair.second;
					crtStructureNodeId = crtPair.first;					
					std::string vkey = boost::lexical_cast<std::string, int> (crtStructureNodeId) + ":" + boost::lexical_cast<std::string, int> (crtSceneNodeId);
					//std::cout << "Stack Pop [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl;
					std::vector<int> tracePrefix = stackTrace.top(); stackTrace.pop();
					std::vector<int> tracePrefixObj = stackTraceObj.top(); stackTraceObj.pop();
					std::vector<std::pair<std::pair<int,int>, int>> tracePrefixFull = stackTraceFull.top(); stackTraceFull.pop();
					visitedstates.clear();	
					for (int j = 0; j < tracePrefixFull.size(); j++) 
					{	
						//crtGraphCut.push_back(tracePrefix[j]); //currentGraphCut Updated.
						//crtGraphCutObj.push_back(tracePrefixObj[j]);
						crtGraphCutObj.push_back(tracePrefixFull[j].first.first);
						crtGraphCut.push_back(tracePrefixFull[j].first.second);
						crtGraphCutObjEdges.push_back(tracePrefixFull[j].second);
						crtGCut->AddNode(nodes->at(tracePrefix[j]));
						visitedstates[tracePrefix[j]] = true;
						crtGraphCutObjNodes.push_back(p_structureNodes->at(tracePrefixObj[j]));
					}
					//Execute actions for transitions so far (if size is greater than 0) ...
					if (crtGraphCut.size() > 0)
						crtGCut->ExecuteActionsFromTrace(crtGraphCutObjNodes, &crtGraphCut, &crtGraphCutObjEdges);

					//Ban options from the previous two paths.
					std::vector<int> altOptions = altClosedPaths.top(); altClosedPaths.pop();
					for (int k = 0; k < altOptions.size(); k++)
					{
						visitedstates[altOptions[k]] = true;
					}					

					visitedpairs[vkey] = true;
					visitedstates[crtSceneNodeId] = true;
					crtGraphCut.push_back(crtSceneNodeId);
					crtGraphCutObj.push_back(crtStructureNodeId);
					crtGCut->AddNode(nodes->at(crtSceneNodeId));
					if (!edgeStack.empty())
					{
						//Execute Actions associated with this move ...
						es = p_structureNodes->at(prevStructureNodeId)->edges->at(edgeStack.top())->first; 
						crtGraphCutObjEdges.push_back(edgeStack.top());
						edgeStack.pop();
						crtGCut->ExecuteActions(es->actions, nodes->at(prevSceneNodeId), nodes->at(crtSceneNodeId));
					}
					else 
					{
						//this is the first pair of states ... push -1 to the obj edges stack
						crtGraphCutObjEdges.push_back(-1);
					}
					//..now check compatibilities and update stack in case of non-deterministic choice.
					while (EdgesCompatible(p_structureNodes->at(crtStructureNodeId), nodes->at(crtSceneNodeId), compatible, edgeIndices))
					{	
						//Remove previosly visited pairs
						tempStore.clear(); int idx = 0;
						for (std::vector<std::pair<int, int>>::iterator compatibleIterator = compatible->begin(); compatibleIterator != compatible->end(); compatibleIterator++, idx++)
						{
							 std::string vk = boost::lexical_cast<std::string, int> (compatibleIterator->first) + ":" + boost::lexical_cast<std::string, int> (compatibleIterator->second);
							 //if (visitedpairs.find(vk) != visitedpairs.end()) tempStore.push_back(idx);
							 if (visitedstates.find(compatibleIterator->second) != visitedstates.end()) tempStore.push_back(idx);
						}
						for (idx = tempStore.size(); idx > 0; idx--)
						{
							compatible->erase(compatible->begin()+tempStore[idx-1]);
							edgeIndices->erase(edgeIndices->begin()+tempStore[idx-1]);
						}
						//Done.


						if (compatible->size() == 0)
						{ 
							//std::cout << "Closing Path [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl; 
							break; 
						}

						if (compatible->size() == 1) 
						{
							//update cut sequence, crtStructureNodeId and crtSceneNodeId.
							prevSceneNodeId = crtSceneNodeId;
							prevStructureNodeId = crtStructureNodeId;							
							crtStructureNodeId = compatible->front().first;
							crtSceneNodeId = compatible->front().second;
							std::string vkey = boost::lexical_cast<std::string, int> (crtStructureNodeId) + ":" + boost::lexical_cast<std::string, int> (crtSceneNodeId);							
							crtGraphCut.push_back(crtSceneNodeId);
							crtGraphCutObj.push_back(crtStructureNodeId);
							crtGraphCutObjEdges.push_back(edgeIndices->front());
							crtGCut->AddNode(nodes->at(crtSceneNodeId));
							visitedpairs[vkey] = true;
							visitedstates[crtSceneNodeId] = true;
							//Carry out the actions linked with this edge
							es = p_structureNodes->at(prevStructureNodeId)->edges->at(edgeIndices->front())->first;							
							crtGCut->ExecuteActions(es->actions, nodes->at(prevSceneNodeId), nodes->at(crtSceneNodeId));
							//std::cout << "One Compatible Edge [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl;
							continue;
						}

						if (compatible->size() > 1) 
						{
							prevSceneNodeId = crtSceneNodeId;
							prevStructureNodeId = crtStructureNodeId;
							//Save state for alt-deterministic choice
							for (int x = 1; x < compatible->size(); x++)
							{
								crtStructureNodeId = compatible->at(x).first;
								crtSceneNodeId = compatible->at(x).second;
								std::string vkey = boost::lexical_cast<std::string, int> (crtStructureNodeId) + ":" + boost::lexical_cast<std::string, int> (crtSceneNodeId);															
								visitedpairs[vkey] = true;
								visitedstates[crtSceneNodeId] = true;
								//std::cout << "Pushing to Stack [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl;
								stack.push(std::pair<int,int>(compatible->at(x).first, compatible->at(x).second));
								stack2.push(std::pair<int,int>(prevStructureNodeId, prevSceneNodeId));								
								edgeStack.push(edgeIndices->at(x));

								std::vector<int> prefixCut;
								std::vector<int> prefixCutObj;
								std::vector<std::pair<std::pair<int,int>, int>> prefixCutFull;
								std::vector<int> altPaths;
								
								for (int x2 = 0; x2 < compatible->size(); x2++)
								{
									if (x2 != x) altPaths.push_back(compatible->at(x2).second);
								}
								altClosedPaths.push(altPaths);
								for (int j = 0; j < crtGraphCut.size(); j++) prefixCut.push_back(crtGraphCut[j]); //currentGraphCut Updated.
								for (int j = 0; j < crtGraphCutObj.size(); j++) prefixCutObj.push_back(crtGraphCutObj[j]); //currentGraphCut Updated.
								for (int j = 0; j < crtGraphCut.size(); j++) prefixCutFull.push_back(std::pair<std::pair<int,int>, int>(std::pair<int,int>(crtGraphCutObj[j], crtGraphCut[j]), crtGraphCutObjEdges[j])); //currentGraphCut Updated.

								stackTrace.push(prefixCut);
								stackTraceObj.push(prefixCutObj);
								stackTraceFull.push(prefixCutFull);
							}

							//Choose first compatible pair, update cut sequence, crtStructureNodeId and crtSceneNodeId.
							crtStructureNodeId = compatible->front().first;
							crtSceneNodeId = compatible->front().second;
							std::string vkey = boost::lexical_cast<std::string, int> (crtStructureNodeId) + ":" + boost::lexical_cast<std::string, int> (crtSceneNodeId);
							crtGraphCut.push_back(crtSceneNodeId);
							crtGraphCutObj.push_back(crtStructureNodeId);
							crtGraphCutObjEdges.push_back(edgeIndices->front());
							crtGCut->AddNode(nodes->at(crtSceneNodeId));
							visitedstates[crtSceneNodeId] = true;
							visitedpairs[vkey] = true;
							//Carry out the actions linked with this edge
							es = p_structureNodes->at(prevStructureNodeId)->edges->at(edgeIndices->front())->first;
							crtGCut->ExecuteActions(es->actions, nodes->at(prevSceneNodeId), nodes->at(crtSceneNodeId));
							//std::cout << "Choosing Path [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl;
							continue;
						}
					}

					//No more compatible edges found in this cut - there might still be some valid cut prefixes in the stack.
					//If this cut is at least greater than 1 node, include in the structures list.
					//Eventually it will be added only if certain conditions are met. e.g. exercising of final states
					if (crtGraphCut.size() > 1 && p_structureNodes->at(crtStructureNodeId)->stats->type == StructureGraph::nodetype::accepting) 
					{
						std::vector<int> * cut = new std::vector<int>();						
						for (std::vector<int>::iterator it = crtGraphCut.begin(); it != crtGraphCut.end(); it++)
						{
							cut->push_back(*it);
						}
						GraphCutsAtNode->push_back(cut);
						crtGCut->orderedCutNodes = cut;
						GCutsAtNode->push_back(crtGCut);
						crtGraphCut.clear();
						crtGraphCutObj.clear();
						crtGraphCutObjEdges.clear();
						crtGraphCutObjNodes.clear();
						crtGCut = new GraphCut(boost::lexical_cast<std::string>(initial));
					} 
					else 
					{
						crtGCut->Clear();
						crtGraphCut.clear();
						crtGraphCutObj.clear();
						crtGraphCutObjEdges.clear();
						crtGraphCutObjNodes.clear();
					}
				}
				//Finish working on this node
				std::vector<std::vector<int>*> * cuts = new std::vector<std::vector<int>*>();
				for (std::vector<std::vector<int>*>::iterator it = GraphCutsAtNode->begin(); it != GraphCutsAtNode->end(); it++)
				{
					cuts->push_back(*it);
				}
				(*structures)[structuresMapKey] = cuts;

				(*structures2)[structuresMapKey] = GCutsAtNode;
				
				GraphCutsAtNode->clear();				
			}			

		}

		return structures2;
	}		
	*/

//Given two nodes, returns pairs of compatible edges.
/*bool EdgeGraph::EdgesCompatible(ObjStructureGraph::Node * p_nodeStructure, StructureGraph::Node * p_nodeScene, std::vector<std::pair<int, int>> * &compatible, std::vector<int> * &edgeIndices) 
{
	compatible->clear();
	edgeIndices->clear();
	//std::cout << "Edges Compatible between " << p_nodeStructure->id << " and " << p_nodeScene->id << std::endl;
	for (int i = 0; i < p_nodeStructure->edges->size(); i++)
	{
		for (int j = 0; j < p_nodeScene->edges->size(); j++)
		{
			if (EdgeCompatible(p_nodeStructure, i, p_nodeScene, j))
			{
				compatible->push_back(std::pair<int,int>(p_nodeStructure->edges->at(i)->second->id,
															p_nodeScene->edges->at(j)->second->id));
				edgeIndices->push_back(i);
				//std::cout << "[" << p_nodeStructure->edges->at(i)->second->id << "," << p_nodeScene->edges->at(j)->second->id << "]" << " Dot Product = " << p_nodeStructure->edges->at(i)->first->dot << std::endl;
			}
		}
	}
	if (compatible->size() > 0) return true;
	return false;
}	

//Check compatibility between p_edge1 of p_node1 and p_edge2 of p_node2
bool StructureGraph::EdgeCompatible(ObjStructureGraph::Node * p_nodeStructure, int p_edge1, StructureGraph::Node * p_nodeScene, int p_edge2) 
{
	bool  dotcompatible = false;
	if ((p_nodeStructure->edges->at(p_edge1)->first->dot+p_nodeStructure->edges->at(p_edge1)->first->epsilon >
		p_nodeScene->edges->at(p_edge2)->first->dot) && (p_nodeScene->edges->at(p_edge2)->first->dot > 
		p_nodeStructure->edges->at(p_edge1)->first->dot-p_nodeStructure->edges->at(p_edge1)->first->epsilon))
		dotcompatible = true;

	return dotcompatible;
}*/